/**************************************************************************************************
* Title: Spring.cpp
* Authors: Gael Huber, Erin Mooney
* Description: Spring object that contains it's own mesh which will be manipulated internally.
**************************************************************************************************/
#include "Spring.h"

/**************************************************************************************************
* Constructor
**************************************************************************************************/
Spring::Spring(void) {
	springConstant = 0.0f;
	naturalLength = Vector3();
	currentLength = Vector3();
	anchorLocal = Vector3();
	anchorGlobal = Vector3();
	mobileAnchor = Vector3();
	mesh = 0;
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
Spring::~Spring(void) {
	if(mesh != 0)
		delete mesh;
}

/**************************************************************************************************
* Apply a force to the spring and update its mesh
**************************************************************************************************/
void Spring::applyForce(Vector3 appliedForce, float dt) {
	// Current force being exerted by the spring internally
	Vector3 internalForce = (currentLength - naturalLength) * springConstant;

	// Find the total force applied to the spring
	Vector3 totalForce = internalForce + appliedForce;
	totalForce *= dt;

	// Find the length by which the spring deviates from the normal length
	Vector3 length = totalForce / springConstant;

	// Update to the newest length of the spring
	currentLength = naturalLength - length;

	// Find the position to move the spring to
	Vector3 stretchPos = currentLength + anchorLocal;

	// Update the mesh values 0,3,6,9
	mesh->vertices[0] = stretchPos;
	mesh->vertices[3] = stretchPos;
	mesh->vertices[6] = stretchPos;
	mesh->vertices[9] = stretchPos;
}

/**************************************************************************************************
* Update the spring to a new position with respect to global space
**************************************************************************************************/
void Spring::updatePositionGlobal(Vector3 newPosition) {
	// Update the current length by subtracting anchor from the new position
	currentLength = newPosition - anchorGlobal;
	
	// The mobile anchor is equivalent to the new position in global coordinates
	mobileAnchor = newPosition;

	// Update the mesh
	Vector3 meshPos = currentLength + anchorLocal;

	// Update the mesh values 0,3,6,9
	mesh->vertices[0] = meshPos;
	mesh->vertices[3] = meshPos;
	mesh->vertices[6] = meshPos;
	mesh->vertices[9] = meshPos;
}

/**************************************************************************************************
* Update the spring to a new position with respect to local space
**************************************************************************************************/
void Spring::updatePositionLocal(Vector3 newPosition) {
	// Update the current length by subtracting anchor from the new position
	currentLength = newPosition - anchorLocal;

	// Update the mobile anchor by using length from the global anchor
	mobileAnchor = currentLength + anchorGlobal;
	
	// Update the mesh values 0,3,6,9
	mesh->vertices[0] = newPosition;
	mesh->vertices[3] = newPosition;
	mesh->vertices[6] = newPosition;
	mesh->vertices[9] = newPosition;
}

/**************************************************************************************************
* Calculate the return the force vector the spring exerts on its non-anchored end
**************************************************************************************************/
Vector3 Spring::getForce(void) {
	// Current force being exerted by the spring internally
	return ((currentLength - naturalLength) * -springConstant);
}

/**************************************************************************************************
* Update using semi-implicit Euler
**************************************************************************************************/
void Spring::update(Vector3 force, float mass, float dt) {
	// Get the internal force
	Vector3 springForce = getForce();

	// Find the total force
	Vector3 total = force + springForce;
	total /= -springConstant;

	// Current length = total force / springconstant
	currentLength = total;

	// Update the mobile anchor
	mobileAnchor = currentLength + anchorGlobal;
	
	// force = -k * x
	// force / -k = x
//	currentLength = force / -springConstant;

//	mobileAnchor = currentLength + anchorGlobal;

//	currentLength = mobileAnchor - anchorGlobal;
	
	/*Vector3 acc = force / mass;
	
	// Find the modifying constant
	float k = springConstant / mass;

	velocity += acc * dt;

	mobileAnchor -= velocity * dt;

	*/
	// Update length
	currentLength = mobileAnchor - anchorGlobal;

/*
	// Update velocity
	velocity.x -= k * k * currentLength.x * dt;  //  -= (currentLength * w.dot(w)) * dt;
	velocity.y -= k * k * currentLength.y * dt;
	velocity.z -= k * k * currentLength.z * dt;

	// Update position
	mobileAnchor.x += velocity.x;
	mobileAnchor.y += velocity.y;
	mobileAnchor.z += velocity.z;

	// Update length
	currentLength = mobileAnchor - anchorGlobal;
*/
	// Update the mesh values 0,3,6,9
	mesh->vertices[0] = mobileAnchor - anchorGlobal;
	mesh->vertices[3] = mobileAnchor - anchorGlobal;
	mesh->vertices[6] = mobileAnchor - anchorGlobal;
	mesh->vertices[9] = mobileAnchor - anchorGlobal;
}